#include "bnao.h"

#include <buola/app/ccmdline.h>

namespace buola { namespace robot { namespace nao {

using namespace buola;

CCmdLineOption<std::string> sIP("ip",L"IP address of NAO","130.237.218.68");
CCmdLineOption<int> sPort("port",L"port of NAO",9559);

BNao::BNao()
    :   BBox("nao")
    ,   oJoints(this,"joints out",26,std::bind(&BNao::OnPullJoints,this))
    ,   mSetGoal(this,"set goal",std::bind(&BNao::SetGoal,this,_1))
    ,   mSpeak(this,"speak",std::bind(&BNao::Speak,this))
    ,   mWalk(this,"walk",std::bind(&BNao::Walk,this))
    ,   mPosture(this,"posture",std::bind(&BNao::Posture,this,_1))
    ,   mRelax(this,"relax",std::bind(&BNao::Relax,this))
    ,   mActual(26)
    ,   mWalking(false)
{
}

void BNao::OnPullJoints()
{
    std::unique_lock<std::mutex> lLock(mMutex);

    msg_info() << "pulled joints\n";

    auto &lProxy=MotionProxy();

    mActual.resize(26);
    std::vector<float> lAngles=lProxy.getAngles("Body",false);
    for(int i=0;i<26;i++)
        mActual[i]=lAngles[i];

    oJoints=mActual;
}

void BNao::SetGoal(const mat::CVecNd<26> &pJoints)
{
    
}

void BNao::Speak()
{
    msg_info() << "should speak\n";
    
    std::unique_lock<std::mutex> lLock(mMutex);

    auto &lProxy=SpeechProxy();
    
    lProxy.say("Hello world");
}

void BNao::Walk()
{
    auto &lMotion=MotionProxy();

    msg_info() << "walking\n";
    lMotion.setWalkArmsEnabled(true,true);
    AL::ALValue lValue;
    lValue.arrayPush("ENABLE_FOOT_CONTACT_PROTECTION");
    lValue.arrayPush(false);
    AL::ALValue lValue2;
    lValue2.arrayPush(lValue);
    lMotion.setMotionConfig(lValue2);
//    lMotion.wbEnable(false);
    start_timer();
    mWalking=!mWalking;
    if(mWalking)
        lMotion.moveToward(0.5,0.0,0.0);
    else
        lMotion.moveToward(0.0,0.0,0.0);
    end_timer();
    msg_info() << "done\n";
}

void BNao::Posture(const std::string &pName)
{
    auto &lMotion=MotionProxy();
    auto &lPosture=PostureProxy();
    msg_info() << "setting posture to " << pName << "\n";
    lMotion.setStiffnesses("Body",1.0);
    lPosture.goToPosture(pName,0.5);
}

void BNao::Relax()
{
    auto &lMotion=MotionProxy();
    msg_info() << "relaxing\n";
    start_timer();
    lMotion.setStiffnesses("Body",0.0);
    end_timer();
}

AL::ALMotionProxy &BNao::MotionProxy()
{
    if(!mMotionProxy)
        mMotionProxy.reset(new AL::ALMotionProxy(cmd_line().GetValue(sIP),cmd_line().GetValue(sPort)));
    return *mMotionProxy;
}

AL::ALTextToSpeechProxy &BNao::SpeechProxy()
{
    if(!mSpeechProxy)
        mSpeechProxy.reset(new AL::ALTextToSpeechProxy(cmd_line().GetValue(sIP),cmd_line().GetValue(sPort)));
    return *mSpeechProxy;
}

AL::ALRobotPostureProxy &BNao::PostureProxy()
{
    if(!mPostureProxy)
        mPostureProxy.reset(new AL::ALRobotPostureProxy(cmd_line().GetValue(sIP),cmd_line().GetValue(sPort)));
    return *mPostureProxy;
}

/*namespace sdh*/ } /*namespace robot*/ } /*namespace buola*/ }
